package com.hitqz.robot.driver.zmq.protocol.handler;

import cn.hutool.core.collection.CollUtil;
import com.example.tutorial.AddressBookProtos;
import com.google.common.collect.Lists;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;

/**
 * 目标点集处理类-自由导航
 *
 * @author wengyx
 * @date 2021-09-14 17:29
 **/
@Slf4j
@Component
public class GoalListHandler implements HandlerZMQService {

    @Override
    public void handlerMsg(String robotSn, byte[] data) throws Exception {
        AddressBookProtos.GoalList goalList = AddressBookProtos.GoalList.parseFrom(data);
        String type = goalList.getDescriptorForType().getName();
        int ackId = goalList.getHeader().getAck();
        int objId = goalList.getHeader().getObj();
        //返回最后一次队列目标点集
        if (objId== AddressBookProtos.GoalList.OBJ_ID.GET_MOVE_BASE_GOAL_LIST_REP_VALUE){
            Map<String, Object> goalListMap = new HashMap<>();
            String taskId = goalList.getTaskId();
            List<AddressBookProtos.NamedPose> namedPoseList = goalList.getDataList();
            goalListMap.put("taskId",taskId);
            LinkedList<PoseDto> list = Lists.newLinkedList();
            for (AddressBookProtos.NamedPose namedPose : CollUtil.emptyIfNull(namedPoseList)) {
                PoseDto pointEntity = new PoseDto();
                pointEntity.setX(Double.valueOf(namedPose.getPose().getPosition().getX()).floatValue());
                pointEntity.setY(Double.valueOf(namedPose.getPose().getPosition().getY()).floatValue());
                pointEntity.setZ(Double.valueOf(namedPose.getPose().getPosition().getZ()).floatValue());
                pointEntity.setYaw(Double.valueOf(namedPose.getPose().getOrientation().getYaw()).floatValue());
                list.addLast(pointEntity);
            }
            goalListMap.put("goalList",list);
            WheelCacheUtil.put(WheelCacheUtil.KEY_MOVE_BASE_GOAL_LIST,goalListMap);
        }
        //释放锁
        WheelToolkit.release(ackId,type,objId,true);
    }

}
